#include "indirect.h"

void ca_conjugate_gradient_dense(double *mna, double *b, int n, double *x, double itol)
{	
	int i,j,iter;
	double rho,rho1,alpha,beta,omega;
	
	double r[n]; 
	double z[n], temp_z[n];
	double q[n], temp_q[n]; 
	double p[n], temp_p[n];
	double res[n];

	double D[n*n];	//Preconditioner
	//double *x;
	//x = *res = (double *)malloc(n*sizeof(double));

	//Initializations		
	memset(D, 0, n*n*sizeof(double));
	memset(r, 0, n*sizeof(double));
	memset(z, 0, n*sizeof(double));
	memset(temp_z, 0, n*sizeof(double));
	memset(q, 0, n*sizeof(double));
	memset(temp_q, 0, n*sizeof(double));
	memset(p, 0, n*sizeof(double));
	memset(temp_p, 0, n*sizeof(double));

	/* Preconditioner (TODO 3exoristi synartisi sto mellon */
	double max;
	
	for(i = 0; i < n; i++){
		max = fabs(mna[i*n]);
		for(j = 0; j < n; j++)
			if(fabs(mna[i*n+j]) > max)
				max = mna[i*n+j];
		D[i*n+i] = max;
	}
	cblas_dcopy (n, x, 1, res, 1);

	cblas_dcopy (n, b, 1, r, 1);
	double r_norm = cblas_dnrm2 (n, r, 1);
	double b_norm = cblas_dnrm2 (n, b, 1);
	if(!b_norm)
		b_norm = 1;

	iter = 0;	
	
	while( r_norm/b_norm > itol && iter < n )
	{
		iter++;

		//SOLVE Mz = r;	-> z = inv(M)*r
		cblas_dcopy (n, r, 1, z, 1);			
		cblas_dtrsv (CblasRowMajor, CblasUpper, CblasNoTrans, CblasNonUnit, n, D, n, z, 1);	// z = MNA*z

		rho = cblas_ddot (n, z, 1, r, 1);
		if (fpclassify(fabs(rho)) == FP_ZERO){
			printf("RHO aborting CG due to EPS...\n");
			exit(42);
		}

		if (iter == 1){
			cblas_dcopy (n, z, 1, p, 1);
		}
		else{		
			beta = rho/rho1;
	
			//p = z + beta*p;
			cblas_dscal (n, beta, p, 1);	//rescale
			cblas_daxpy (n, 1, z, 1, p, 1);	//p = 1*z + p
			
		}		
		rho1 = rho;
		
		//q = Ap
		cblas_dgemv (CblasRowMajor,CblasNoTrans,n,n,1,mna,n,p,1,0,q,1);

		omega = cblas_ddot (n, p, 1, q, 1);
		if (fpclassify(fabs(omega)) == FP_ZERO){
			printf("OMEGA aborting CG due to EPS...\n");
			exit(42);
		}

		alpha = rho/omega;	

		//x = x + aplha*p;
		cblas_dcopy (n, p, 1, temp_p, 1);
		cblas_dscal (n, alpha, temp_p, 1);//rescale by alpha
		cblas_daxpy (n, 1, temp_p, 1, res, 1);// sum x = 1*x + temp_p

		//r = r - aplha*q;
		cblas_dcopy (n, q, 1, temp_q, 1);
		cblas_dscal (n, -alpha, temp_q, 1);//rescale by alpha
		cblas_daxpy (n, 1, temp_q, 1, r, 1);// sum r = 1*r - temp_p

		//next step
		r_norm = cblas_dnrm2 (n, r, 1);
	}
	cblas_dcopy (n, res, 1, x, 1);
}

void ca_conjugate_gradient_sparse(cs *mna, double *b, int n, double *x, double itol)
{	
	int i,j,iter;
	double rho,rho1,alpha,beta,omega;
	
	double r[n]; 
	double z[n], temp_z[n];
	double q[n], temp_q[n]; 
	double p[n], temp_p[n];
	double res[n];

	double D[n*n];	//Preconditioner
	//Initializations		
	memset(D, 0, n*n*sizeof(double));
	memset(r, 0, n*sizeof(double));
	memset(z, 0, n*sizeof(double));
	memset(temp_z, 0, n*sizeof(double));
	memset(q, 0, n*sizeof(double));
	memset(temp_q, 0, n*sizeof(double));
	memset(p, 0, n*sizeof(double));
	memset(temp_p, 0, n*sizeof(double));

	/* Preconditioner */
	double max;
	int pp;
	for(j = 0; j < n; ++j){
		for(pp = mna->p[j], max = fabs(mna->x[pp]); pp < mna->p[j+1]; pp++)
			if(fabs(mna->x[pp]) > max)
				max = fabs(mna->x[pp]);
		D[j*n+j] = max;
	}	
	cblas_dcopy (n, x, 1, res, 1);

	cblas_dcopy (n, b, 1, r, 1);
	double r_norm = cblas_dnrm2 (n, r, 1);
	double b_norm = cblas_dnrm2 (n, b, 1);
	if(!b_norm)
		b_norm = 1;

	iter = 0;	
	
	while( r_norm/b_norm > itol && iter < n )
	{
		iter++;

		//SOLVE Mz = r;	-> z = inv(M)*r
		cblas_dcopy (n, r, 1, z, 1);			
		cblas_dtrsv (CblasRowMajor, CblasUpper, CblasNoTrans, CblasNonUnit, n, D, n, z, 1);	// z = MNA*z

		rho = cblas_ddot (n, z, 1, r, 1);
		if (fpclassify(fabs(rho)) == FP_ZERO){
			printf("RHO aborting CG due to EPS...\n");
			exit(42);
		}

		if (iter == 1){
			cblas_dcopy (n, z, 1, p, 1);
		}
		else{		
			beta = rho/rho1;
	
			//p = z + beta*p;
			cblas_dscal (n, beta, p, 1);	//rescale
			cblas_daxpy (n, 1, z, 1, p, 1);	//p = 1*z + p
			
		}		
		rho1 = rho;
		
		//q = Ap
		memset(q, 0, n*sizeof(double));
		cs_gaxpy (mna, p, q);

		omega = cblas_ddot (n, p, 1, q, 1);
		if (fpclassify(fabs(omega)) == FP_ZERO){
			printf("OMEGA aborting CG due to EPS...\n");
			exit(42);
		}

		alpha = rho/omega;	

		//x = x + aplha*p;
		cblas_dcopy (n, p, 1, temp_p, 1);
		cblas_dscal (n, alpha, temp_p, 1);//rescale by alpha
		cblas_daxpy (n, 1, temp_p, 1, res, 1);// sum x = 1*x + temp_p

		//r = r - aplha*q;
		cblas_dcopy (n, q, 1, temp_q, 1);
		cblas_dscal (n, -alpha, temp_q, 1);//rescale by alpha
		cblas_daxpy (n, 1, temp_q, 1, r, 1);// sum r = 1*r - temp_p

		//next step
		r_norm = cblas_dnrm2 (n, r, 1);
	}
	cblas_dcopy (n, res, 1, x, 1);

}
void ca_bi_conjugate_gradient_dense(double *mna, double *b, int n, double *x, double itol)
{
	
	int i,j,iter;
	
	double rho,rho1,alpha,beta,omega;
	
	double r[n], r_t[n]; 
	double z[n], z_t[n], temp_z[n];
	double q[n], q_t[n], temp_q[n]; 
	double p[n], p_t[n], temp_p[n];
	
	double D[n*n];	//Preconditioner
	
	//Initializations		
	memset(D, 0, n*n*sizeof(double));
	memset(r, 0, n*sizeof(double));
	memset(r_t, 0, n*sizeof(double));
	memset(z, 0, n*sizeof(double));
	memset(z_t, 0, n*sizeof(double));
	memset(temp_z, 0, n*sizeof(double));
	memset(q, 0, n*sizeof(double));
	memset(q_t, 0, n*sizeof(double));
	memset(temp_q, 0, n*sizeof(double));
	memset(p, 0, n*sizeof(double));
	memset(p_t, 0, n*sizeof(double));
	memset(temp_p, 0, n*sizeof(double));

	/* Preconditioner */
	double max;
	for(i = 0; i < n; i++){
		max = fabs(mna[i*n]);
		for(j = 0; j < n; j++)
			if(fabs(mna[i*n+j]) > max)
				max = mna[i*n+j];
		D[i*n+i] = max;
	}


	cblas_dcopy (n, b, 1, r, 1);
	cblas_dcopy (n, b, 1, r_t, 1);
	double r_norm = cblas_dnrm2 (n, r, 1);
	double b_norm = cblas_dnrm2 (n, b, 1);
	if(!b_norm)
		b_norm = 1;

	iter = 0;

	while( r_norm/b_norm > itol && iter < n )
	{
		iter++;

		//SOLVE Mz = r;	-> z = inv(M)*r
		cblas_dcopy (n, r, 1, z, 1);		
		cblas_dtrsv (CblasRowMajor, CblasUpper, CblasNoTrans, CblasNonUnit, n, D, n, z, 1);	// z = MNA*z

		cblas_dcopy (n, r_t, 1, z_t, 1);			
		cblas_dtrsv (CblasRowMajor, CblasUpper, CblasTrans, CblasNonUnit, n, D, n, z_t, 1); // z_t = MNA*z_t	
		
		rho = cblas_ddot (n, z, 1, r_t, 1);		
		if (fpclassify(fabs(rho)) == FP_ZERO){
			printf("RHO aborting Bi-CG due to EPS...\n");
			exit(42);
		}
		
		if (iter == 1){
			cblas_dcopy (n, z, 1, p, 1);
			cblas_dcopy (n, z_t, 1, p_t, 1);
		}
		else{		
			//p = z + beta*p;
			beta = rho/rho1;			

			cblas_dscal (n, beta, p, 1);	//rescale p by beta
			cblas_dscal (n, beta, p_t, 1);
		
			cblas_daxpy (n, 1, z, 1, p, 1);//p = 1*z + p
			cblas_daxpy (n, 1, z_t, 1, p_t, 1);//p = 1*z_t + p
		}
		
		rho1 = rho;
		
		//q = Ap
		cblas_dgemv (CblasRowMajor,CblasNoTrans,n,n,1,mna,n,p,1,0,q,1);

		//q_t = trans(A)*p_t		
		cblas_dgemv (CblasRowMajor,CblasTrans,n,n,1,mna,n,p_t,1,0,q_t,1);		

		omega = cblas_ddot (n, p_t, 1, q, 1);
		if (fpclassify(fabs(omega)) == FP_ZERO){
			printf("OMEGA aborting Bi-CG due to EPS...\n");
			exit(42);
		}

		alpha = rho/omega;		

		//x = x + aplha*p;
		cblas_dcopy (n, p, 1, temp_p, 1);
		cblas_dscal (n, alpha, temp_p, 1);//rescale by aplha
		cblas_daxpy (n, 1, temp_p, 1, x, 1);// sum x = 1*x + temp_p

		//R = R - aplha*Q;
		cblas_dcopy (n, q, 1, temp_q, 1);
		cblas_dscal (n, -alpha, temp_q, 1);//rescale by -aplha
		cblas_daxpy (n, 1, temp_q, 1, r, 1);// sum r = 1*r - temp_p		

		//~r=~r-alpha*~q
		cblas_dcopy (n, q_t, 1, temp_q, 1);
		cblas_dscal (n, -alpha, temp_q, 1);//rescale by -aplha
		cblas_daxpy (n, 1, temp_q, 1, r_t, 1);// sum r = 1*r - temp_p

		r_norm = cblas_dnrm2 (n, r, 1);	//next step
	}
}

void ca_bi_conjugate_gradient_sparse(cs *mna, double *b, int n, double *x, double itol)
{
	int i,j,iter;
	
	double rho,rho1,alpha,beta,omega;
	
	double r[n], r_t[n]; 
	double z[n], z_t[n], temp_z[n];
	double q[n], q_t[n], temp_q[n]; 
	double p[n], p_t[n], temp_p[n];
	
	double D[n*n];
	
	//Initializations		
	memset(D, 0, n*n*sizeof(double));
	memset(r, 0, n*sizeof(double));
	memset(r_t, 0, n*sizeof(double));
	memset(z, 0, n*sizeof(double));
	memset(z_t, 0, n*sizeof(double));
	memset(temp_z, 0, n*sizeof(double));
	memset(q, 0, n*sizeof(double));
	memset(q_t, 0, n*sizeof(double));
	memset(temp_q, 0, n*sizeof(double));
	memset(p, 0, n*sizeof(double));
	memset(p_t, 0, n*sizeof(double));
	memset(temp_p, 0, n*sizeof(double));
	
	//Preconditioner
	double max;
	int pp;
	for(j = 0; j < n; ++j){
		for(pp = mna->p[j], max = fabs(mna->x[pp]); pp < mna->p[j+1]; pp++)
			if(fabs(mna->x[pp]) > max)
				max = fabs(mna->x[pp]);
		D[j*n+j] = max;
	}
	cs *mnaT = cs_transpose (mna, 1) ;


	cblas_dcopy (n, b, 1, r, 1);
	cblas_dcopy (n, b, 1, r_t, 1);
	double r_norm = cblas_dnrm2 (n, r, 1);
	double b_norm = cblas_dnrm2 (n, b, 1);
	if(!b_norm)
		b_norm = 1;
	
	iter = 0;

	while( r_norm/b_norm > itol && iter < n )
	{
		iter++;

		//SOLVE Mz = r;	-> z = inv(M)*r
		cblas_dcopy (n, r, 1, z, 1);		
		cblas_dtrsv (CblasRowMajor, CblasUpper, CblasNoTrans, CblasNonUnit, n, D, n, z, 1);	// z = MNA*z

		cblas_dcopy (n, r_t, 1, z_t, 1);			
		cblas_dtrsv (CblasRowMajor, CblasUpper, CblasTrans, CblasNonUnit, n, D, n, z_t, 1); // z_t = MNA*z_t	
		
		rho = cblas_ddot (n, z, 1, r_t, 1);		
		if (fpclassify(fabs(rho)) == FP_ZERO){
			printf("RHO aborting Bi-CG due to EPS...\n");
			exit(42);
		}
		
		if (iter == 1){
			cblas_dcopy (n, z, 1, p, 1);
			cblas_dcopy (n, z_t, 1, p_t, 1);
		}
		else{		
			//p = z + beta*p;
			beta = rho/rho1;			

			cblas_dscal (n, beta, p, 1);	//rescale p by beta
			cblas_dscal (n, beta, p_t, 1);
		
			cblas_daxpy (n, 1, z, 1, p, 1);//p = 1*z + p
			cblas_daxpy (n, 1, z_t, 1, p_t, 1);//p = 1*z_t + p
		}
		
		rho1 = rho;
		
		//q = Ap
		//q_t = trans(A)*p_t
		memset(q, 0, n*sizeof(double));
		cs_gaxpy (mna, p, q);
		memset(q_t, 0, n*sizeof(double));
		cs_gaxpy(mnaT, p_t, q_t);			
		
		omega = cblas_ddot (n, p_t, 1, q, 1);
		if (fpclassify(fabs(omega)) == FP_ZERO){
			printf("OMEGA aborting Bi-CG due to EPS...\n");
			exit(42);
		}

		alpha = rho/omega;		

		//x = x + aplha*p;
		cblas_dcopy (n, p, 1, temp_p, 1);
		cblas_dscal (n, alpha, temp_p, 1);//rescale by aplha
		cblas_daxpy (n, 1, temp_p, 1, x, 1);// sum x = 1*x + temp_p

		//R = R - aplha*Q;
		cblas_dcopy (n, q, 1, temp_q, 1);
		cblas_dscal (n, -alpha, temp_q, 1);//rescale by -aplha
		cblas_daxpy (n, 1, temp_q, 1, r, 1);// sum r = 1*r - temp_p		

		//~r=~r-alpha*~q
		cblas_dcopy (n, q_t, 1, temp_q, 1);
		cblas_dscal (n, -alpha, temp_q, 1);//rescale by -aplha
		cblas_daxpy (n, 1, temp_q, 1, r_t, 1);// sum r = 1*r - temp_p

		r_norm = cblas_dnrm2 (n, r, 1);	//next step
	}
	
	//cs_spfree(mnaT);
}
